The X-Philes (2nd Revision)

home *** CD-ROM | disk | FTP | other *** search

/ The X-Philes (2nd Revision) / The X-Philes Number 1 (1995).iso / xphiles / hp48hor1 / lqpoles.src < prev next >

Wrap

Text File | 1991-02-21 | 12KB | 716 lines

%%HP: T(3)A(D)F(.); @ Optimal root placement for Linear-Quadratic Optimum Controll @ LQPOLES @ by James Walters DIR PG4 DIR MAIN DIR EE DIR CNTRL DIR STVR DIR @ For the optimal pole assignment program, begin with a system @ dX/dt = A*X+B*u, y = C*X+D*u @ and a cost function J = integral( TRN(X)*Q*X + R*u^2) @ to be minimized. @ Store A, B, C, D, Q, and R matrices as variables. @ @ run OPRTS. @ On the stack will be the optimal feedback gain matrix g. @ The optimal closed loop system is of the form @ dX/dt = A*X + B*(-g*X) @ New variables Qo, Qc, H, W, a, ahat, n are also formed by this program @ @ Included below is both the main program, and a sample system @ @ The optimal roots should be -35, -91.6, -12.57 @ OPRTS @ OPTIMAL ROOTS PLACEMENT \<< HAMLTN @ form hamiltonian matrix CHEQ ROOTS 2 n * @ find roots of hamiltonian \->LIST \-> HROOTS \<< 1 n 2 * FOR I @ keep roots with negative real parts HROOTS I GET DUP IF RE 0 > THEN DROP END NEXT \>> n \->LIST @ place optimal roots into list IRT LTOV 'ahat' STO @ transform optimal roots into polynomial vector ahat POLEPL g @ place roots of system at optimal position, return optimal \>> @ gain matrix on stack. POLEPL @ POLE PLACEMENT PROGRAM \<< A CHEQ @ find characteristic equation of system DUP SIZE 1 - 'n' STO DUP LTOV 'a' @ place eq in vector form STO n IDN 'W' STO 2 @ form wronskian matrix W n FOR i DUP i GET i n FOR j DUP 'W(j-i+1,j)' STO NEXT DROP NEXT DUP CNTRLBL DROP Qc W * @ do controllability test, generate Qc INV TRN ahat a - * @ determine gain matrix g to place poles at desired EVAL TRN 'g' STO @ location. Store gain matrix as g. \>> OBSVBL @ OBSERVABILITY MATRIX PROGRAM \<< n IDN DUP 'Qo' STO 1 n FOR j DUP C TRN * 1 n FOR i DUP i 1 2 \->LIST GET Qo SWAP i j 2 \->LIST SWAP PUT 'Qo' STO NEXT DROP A TRN * NEXT DROP Qo DET 0 \=/ \>> CNTRLBL @ CONTROLLABILITY MATRIX PROGRAM \<< n IDN DUP 'Qc' STO 1 n FOR j DUP B * 1 n FOR i DUP i 1 2 \->LIST GET Qc SWAP i j 2 \->LIST SWAP PUT 'Qc' STO NEXT DROP A * NEXT DROP Qc DET 0 \=/ \>> HAMLTN @ HAMILTONIAN MATRIX PROGRAM \<< B R INV * B TRN * -1 * \-> tmp \<< n 2 * IDN 'H' STO H 1 n FOR i 1 n FOR j A i j 2 \->LIST GET i j 2 \->LIST SWAP PUT Q i j 2 \->LIST GET -1 * i n + j 2 \->LIST SWAP PUT A j i 2 \->LIST GET -1 * i n + j n + 2 \->LIST SWAP PUT tmp i j 2 \->LIST GET i j n + 2 \->LIST SWAP PUT NEXT NEXT 'H' STO H \>> \>> LTOV @ LIST TO COLUMN VECTOR PROGRAM \<< DUP SIZE 1 - DUP 1 2 \->LIST 0 CON \-> n a \<< 2 n 1 + FOR i DUP i GET RE 'a(i-1,1)' STO NEXT DROP a \>> \>> H @ EXAMPLE HAMILTONIAN MATRIX [[ 91 2 3 -1 -2 -3 ] [ 4 5 65 -2 -4 -6 ] [ 7 8 19 -3 -6 -9 ] [ -1 0 0 -91 -4 -7 ] [ 0 -1 0 -2 -5 -8 ] [ 0 0 -1 -3 -65 -19 ]] Q @ EXAMPLE STATE VARIABLE WEIGHTING MATRIX [[ 1 0 0 ] [ 0 1 0 ] [ 0 0 1 ]] R 1 @ EXAMPLE CONTROL WEIGHT g @ EXAMPLE STATE VARIBALE FEEDBACK MATRIX [[ 335.453288908 -2.61764442827 -25.3230917657 ]] W @ EXAMPLE WRONSKIAN MATRIX [[ 1 -115 1730 ] [ 0 1 -115 ] [ 0 0 1 ]] a @ EXAMPLE SYSTEM CHARACTERISTIC VECTOR [[ -115 ] [ 1730 ] [ 37926 ]] ahat @ EXAMPLE OPTIMAL CHARACTERISTIC VECTOR [[ 139.248724755 ] [ 4805.60367291 ] [ 40398.0276333 ]] Qo @ EXAMPLE OBSERVABILITY MATRIX [[ 1 8 132 ] [ 0 10 162 ] [ 1 12 192 ]] C @ C MATRIX OF y = C*X+D*u [[ 1 0 1 ]] Qc @ EXAMPLE CONTROLLABILITY MATRIX [[ 1 104 10122 ] [ 2 209 6661 ] [ 3 80 3920 ]] n 3 @ SYSTEM ORDER B @ dXdt = A*X+B*u [[ 1 ] [ 2 ] [ 3 ]] A @ dXdt = A*X+B*u [[ 91 2 3 ] [ 4 5 65 ] [ 7 8 19 ]] END @ END OF STATE VARIABLE DIRECTORY END @ END OF CNTRL DIRECTORY END @ END OF EE DIR END @ END OF MAIN DIR CHEQ @ FINDS CHARACTERISTIC EQUATION OF A MATRIX \<< TRCN SYM \>> LVCT @ \<< \-> n \<< n 1 \->LIST 0 CON n 1 FOR i i 1 \->LIST 3 ROLL PUT -1 STEP \>> \>> GSO \<< DUP SIZE LIST\-> DROP DUP DUP 2 + ROLLD \->LIST \-> M \<< 2 SWAP FOR n M n GET 1 n 1 - FOR i M i GET DUP DUP2 DOT INV * SWAP 3 PICK DOT * - NEXT n M SWAP ROT PUT 'M' STO NEXT M LIST\-> DROP \>> \>> TRCN \<< DUP SIZE 1 GET \-> g n \<< g 'tmp' STO { } 1 n START 0 1 n FOR i tmp i DUP 2 \->LIST GET + NEXT 1 \->LIST + 'tmp' g STO* NEXT 'tmp' PURGE \>> \>> SYM \<< DUP SIZE \-> b n \<< { 1 } 1 n FOR i \-> s \<< 0 1 i FOR j b j GET s i j - 1 + GET * - NEXT i / 1 \->LIST s SWAP + \>> NEXT \>> \>> PSERS \<< \-> X \<< LIST\-> 0 SWAP 1 FOR n n 1 + ROLL X n 1 - ^ * + -1 STEP \>> \>> CST { UP MAIN EXCO ROOTS PADD PMUL PDIV SYM\-> \->SYM IRT CST PG5 CHEQ \->VCT } END @ END OF PG4 DIRECTORY ROOTS @ Finds roots of any polynomial \<< TRIM DUP SIZE \-> n \<< IF n 5 > THEN DUP BAIRS SWAP OVER PDIV DROP \-> A B \<< A ROOTS B ROOTS \>> ELSE PROOT END \>> \>> EXCO @ Expand completely algebraic \<< \<< EXPAN \>> MULTI \<< COLCT \>> MULTI \>> PADD @ Adds to polynomial lists \<< DUP2 SIZE SWAP SIZE \-> A B nB nA \<< A L\178A B L\178A IF nA nB < THEN SWAP END IF nA nB \=/ THEN 1 nA nB - ABS START 0 NEXT END nA nB - ABS 1 + ROLL OBJ\-> 1 GET nA nB - ABS + \->ARRY + L\178A \>> \>> PMUL @ Multiplies polynomial lists \<< DUP2 SIZE SWAP SIZE \-> A B nB nA \<< { } IF nB 1 > THEN 2 nB START 0 + NEXT END DUP 'A' STO+ 'A' SWAP STO+ A OBJ\-> \->ARRY B OBJ\-> DROP IF nB 1 > THEN 2 nB FOR J J ROLL NEXT END IF 3 nA nB + \<= THEN 3 nA nB + START 0 NEXT END nA nB 1 - 2 * + \->ARRY 2 nA nB + START DUP2 DOT 3 ROLLD OBJ\-> SWAP nA nB 1 - 2 * + 1 + ROLLD \->ARRY NEXT DROP2 nA nB + 1 - \->LIST \>> \>> PDIV \<< DUP SIZE 3 ROLLD OBJ\-> \->ARRY SWAP OBJ\-> \->ARRY \-> c b a \<< a b IF c 1 SAME THEN OBJ\-> DROP / OBJ\-> 1 GET \->LIST { 0 } ELSE WHILE OVER SIZE 1 GET c \>= REPEAT DIVV END DROP \-> d \<< a SIZE 1 GET c 1 - - \->LIST d OBJ\-> OBJ\-> DROP \->LIST \>> END \>> \>> SYML \<< 0 \-> E n \<< DO 0 'X' STO E EVAL n ! / 'X' PURGE 1 'n' STO+ UNTIL E 'X' \.d DUP 'E' STO IF TYPE 0 == THEN IF E 0 == THEN 1 ELSE 0 END ELSE 0 END END 2 n FOR i i ROLL NEXT n \->LIST \>> \>> LSYM \<< 'X' \-> x \<< LIST\-> 0 SWAP 1 FOR n n 1 + ROLL x n 1 - ^ * + -1 STEP \>> \>> IRT \<< OBJ\-> \-> n \<< IF n 0 > THEN 1 n START n ROLL { 1 } SWAP NEG + NEXT ELSE { 1 } END IF n 1 > THEN 2 n START PMUL NEXT END \>> \>> MULTI \<< \-> p \<< DO DUP p EVAL UNTIL DUP ROT SAME END \>> \>> PROOT \<< LIST\-> \->ARRY DUP 1 GET / ARRY\-> LIST\-> DROP 1 - DUP 2 + ROLL DROP { NEG QUD CUBIC QUAR } SWAP GET EVAL \>> QUAR \<< 3 PICK NEG DUP 6 ROLLD 5 PICK 4 PICK * 3 PICK 4 * - 5 ROLL 4 * 6 PICK SQ - 4 PICK * 5 PICK SQ - CUBIC 3 \->LIST 1 3 FOR n DUP n GET 2 / SQ n 2 + PICK - ABS SWAP NEXT 4 ROLLD DUP2 IF < THEN SWAP DROP 3 ELSE DROP 2 END 3 ROLLD IF > THEN DROP 1 END GET 4 ROLL 2 / SWAP 2 / DUP SQ 4 ROLL - \v/ IF DUP ABS THEN 3 DUPN 3 ROLLD * 6 ROLL 2 / - SWAP / ELSE 3 PICK SQ 6 ROLL + 3 PICK 2 * + \v/ END 5 ROLL DROP 3 ROLLD 4 DUPN + 3 ROLLD + SWAP QUD 6 ROLLD 6 ROLLD - 3 ROLLD - SWAP QUD \>> CUBIC \<< 3 PICK -3 / 3 PICK 5 PICK SQ 3 / - 5 ROLL DUP 3 ^ 2 * SWAP 9 * 6 ROLL * - 27 / 4 ROLL + NEG OVER ABS 0 IF == THEN 3 INV ^ SWAP DROP 0 SWAP ELSE 2 / DUP SQ 3 PICK 3 ^ 27 / + \v/ - 3 INV ^ SWAP OVER / 3 / NEG END -1 3 \v/ R\->C 2 / 4 ROLLD 3 DUPN 3 DUPN + + 8 ROLLD 7 PICK * SWAP 7 PICK / + + 5 ROLLD 4 PICK / SWAP 4 ROLL * + + \>> TRIM \<< OBJ\-> \-> n \<< n WHILE ROLL DUP NOT n 1 - AND REPEAT DROP 'n' DECR END n ROLLD n \->LIST \>> \>> RDER \<< \-> F G \<< G F PDER PMUL G PDER { -1 } PMUL F PMUL PADD G G PMUL \>> \>> PDER \<< OBJ\-> \-> n \<< 1 n FOR i n ROLL n i - * NEXT DROP IF n 1 == THEN { 0 } ELSE n 1 - \->LIST END \>> \>> PF \<< MAXR { } \-> Z P OLD LAST \<< 1 P SIZE FOR I P I GET \-> p1 \<< IF p1 OLD \=/ THEN Z p1 EVPLY 1 P SIZE FOR J IF P J GET P I GET \=/ THEN p1 P J GET - / END NEXT p1 'OLD' STO { } 'LAST' STO ELSE IF { } LAST SAME THEN 1 { } 1 P SIZE FOR K P K GET IF DUP p1 == THEN DROP ELSE + END NEXT IRT Z SWAP ELSE LAST OBJ\-> DROP END RDER DUP2 5 PICK 1 + 3 ROLLD 3 \->LIST 'LAST' STO p1 EVPLY SWAP p1 EVPLY SWAP / SWAP ! / END \>> NEXT P SIZE \->LIST \>> \>> FCTP \<< IF DUP SIZE 3 > THEN DUP BAIRS SWAP OVER PDIV DROP FCTP END \>> L\178A \<< IF DUP TYPE 5 == THEN OBJ\-> \->ARRY ELSE OBJ\-> 1 GET \->LIST END \>> EVPLY \<< OVER IF DUP TYPE 5 == THEN SIZE ELSE SIZE 1 GET END \-> a r n \<< a 1 GET IF n 1 > THEN 2 n FOR i r * a i GET + NEXT END \>> \>> COEF \<< \-> E n \<< 0 n FOR I 0 'X' STO E EVAL 'X' PURGE E 'X' \.d 'E' STO I ! / NEXT 2 n 1 + FOR I I ROLL NEXT n 1 + \->LIST \>> \>> EQ 1 DIVV \<< DUP 1 GET \-> a b c \<< a 1 GET c / DUP b * a SIZE RDM a SWAP - OBJ\-> 1 GETI 1 - PUT \->ARRY SWAP DROP b \>> \>> QUD \<< SWAP 2 / NEG DUP SQ ROT - \v/ DUP2 + 3 ROLLD - \>> BAIRS \<< OBJ\-> 1 1 \-> n R S \<< DO 0 n 1 + PICK 0 0 0 4 PICK 5 n + 7 FOR J J PICK R 7 PICK * + S 8 PICK * + 7 ROLL DROP DUP 6 ROLLD R 3 PICK * + S 4 PICK * + 5 ROLL DROP -1 STEP 3 PICK SQ 3 PICK 6 PICK * - IF DUP 0 == THEN DROP 1 1 ELSE 6 PICK 6 PICK * 5 PICK 9 PICK * - OVER / 4 PICK 9 PICK * 8 PICK 7 PICK * - ROT / END DUP 'S' STO+ SWAP DUP 'R' STO+ UNTIL R\->C ABS .000000001 < 7 ROLLD 6 DROPN END n DROPN 1 R NEG S NEG 3 \->LIST \>> \>> CST { UP EXCO ROOTS PADD PMUL PDIV SYML LSYM IRT } END @ END OF PG3 DIRECTORY